#include <cstdio>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "epuckDriver/OdometryPublisherMsg.h"
#include "epuckDriver/IRDataPublisherMsg.h"
#include "epuckDriver/CameraDataMsg.h"

#include <unistd.h>

ros::Publisher pub;

void VelocityCallBack(const geometry_msgs::Twist::ConstPtr& twist){
    pub.publish(twist);     
}

void OdometryCallBack(const
epuckDriver::OdometryPublisherMsg::ConstPtr& odom){

   ROS_INFO("Odometry"); 
   ROS_INFO("position[x,y,theta]: %lf %lf %lf\n",
           odom->pose2d.x, odom->pose2d.y, odom->pose2d.theta); 
   ROS_INFO("velocity[x,y,theta]: %lf %lf %lf\n",        
           odom->twist.linear.x, odom->twist.linear.y, odom->twist.angular.z); 
   ROS_INFO("-------------------------------------------");
}

void IRCallBack(const 
epuckDriver::IRDataPublisherMsg::ConstPtr& ir){

    ROS_INFO("IR");
    ROS_INFO("voltages_count: %d,ranges_count: %d",ir->voltages_count,ir->ranges_count);
    int number = ir->voltages_count;
    for(int i =0;i < number; i++){
        ROS_INFO("voltages[%d]: %lf ranges[%d]: %lf",i,ir->voltages[i],i,ir->ranges[i]);
    }    
    ROS_INFO("-------------------------------------------");
  
}

void CameraCallBack(const 
epuckDriver::CameraDataMsg::ConstPtr& camera){
    
    ROS_INFO("Camera");
    ROS_INFO("width: %d height: %d",camera->width,camera->height);
    ROS_INFO("bpp: %d format: %d",camera->bpp,camera->format);
    ROS_INFO("fdiv: %d compression: %d",camera->fdiv,camera->compression);     
    ROS_INFO("image_count: %d",camera->image_count);
    ROS_INFO("-------------------------------------------");
}

int main(int argc,char **argv){

    ros::init(argc,argv,"testVelocity");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe<geometry_msgs::Twist>("cmd_vel",1,VelocityCallBack);
    pub = n.advertise<geometry_msgs::Twist>("epuckVelocity1",1);    
    
    ros::Subscriber subO =
n.subscribe<epuckDriver::OdometryPublisherMsg>("epuckOdometry1",1,OdometryCallBack);
    ros::Subscriber subI =
n.subscribe<epuckDriver::IRDataPublisherMsg>("epuckIR1",1,IRCallBack);
//    ros::Subscriber subC =
//n.subscribe<epuckDriver::CameraDataMsg>("epuckCamera1",1,CameraCallBack);

    while(true)        
        ros::spinOnce();
    
    return 0;
}
